import numpy as np
def InterPos_2(q0, qlist = [], vqmax = 0.1, dt = 0.01):
    '''
    本函数目标是根据期望的关节空间坐标轨迹qlist，插值得到平滑的关节空间坐标轨迹
    输入q0为当前的位置关节空间坐标
    输入一个qlist列表，里面每一项都是所规划路径上的点所对应的关节空间坐标，每个元素都是array。
    输入vqmax为每个关节的最大速度
    通过限制delta_q的无穷范数（最大分量值），限制每个delta_q的分量
    '''
    norm_inf_max = vqmax*dt
    qlist_inter = []
    q_start = q0 # 每次循环开始的关节空间坐标
    for qdes in qlist:
        delta_q = qdes - q_start
        new_delta_q = delta_q
        while np.linalg.norm(delta_q, ord=np.inf) > norm_inf_max: # 判断是否需要插值
            # delta_q = delta_q/np.linalg.norm(delta_q, ord=np.inf)*norm_inf_max #如果需要插值，就获得新的更小的delta_q
            for i, delta_q_i in enumerate(delta_q):
                if np.abs(delta_q_i) > norm_inf_max:
                    delta_q_i = norm_inf_max*np.sign(delta_q_i)
                new_delta_q[i] = delta_q_i

            qlist_inter.append(q_start + new_delta_q)
            q_start += new_delta_q
            delta_q = qdes - q_start # 更新delta_q
        qlist_inter.append(qdes)
        q_start = qdes
    
    return qlist_inter